Technical  Research  Report 


Motion  Control  of  Drift-Free,  Left-Invariant 
Systems  on  Lie  Groups 


by  N.E.  Leonard,  P.S.  Krishnaprasad 


T.R.  94-8 


Sponsored  by 

the  National  Science  Foundation 
Engineering  Research  Center  Program, 
the  University  of  Maryland, 

Harvard  University, 
and  Industry 


Report  Documentation  Page 

Form  Approved 

OMB  No.  0704-0188 

Public  reporting  burden  for  the  collection  of  information  is  estimated  to  average  1  hour  per  response,  including  the  time  for  reviewing  instructions,  searching  existing  data  sources,  gathering  and 
maintaining  the  data  needed,  and  completing  and  reviewing  the  collection  of  information.  Send  comments  regarding  this  burden  estimate  or  any  other  aspect  of  this  collection  of  information, 
including  suggestions  for  reducing  this  burden,  to  Washington  Headquarters  Services,  Directorate  for  Information  Operations  and  Reports,  1215  Jefferson  Davis  Highway,  Suite  1204,  Arlington 

VA  22202-4302.  Respondents  should  be  aware  that  notwithstanding  any  other  provision  of  law,  no  person  shall  be  subject  to  a  penalty  for  failing  to  comply  with  a  collection  of  information  if  it 
does  not  display  a  currently  valid  OMB  control  number. 

1.  REPORT  DATE 

SEP  1995 

2.  REPORT  TYPE 

3.  DATES  COVERED 

00-00-1995  to  00-00-1995 

4.  TITLE  AND  SUBTITLE 

5a.  CONTRACT  NUMBER 

Motion  Control  of  Drift-Free,  Left-Invariant  Systems  on  Lie  Groups 

5b.  GRANT  NUMBER 

5c.  PROGRAM  ELEMENT  NUMBER 

6.  AUTHOR(S) 

5d.  PROIECT  NUMBER 

5e.  TASK  NUMBER 

5f.  WORK  UNIT  NUMBER 

7.  PERFORMING  ORGANIZATION  NAME(S)  AND  ADDRESS(ES) 

Department  of  Electrical  Engineering, Institute  for  Systems 

Research, University  of  Maryland, College  Park, MD, 20742 

8.  PERFORMING  ORGANIZATION 

REPORT  NUMBER 

9.  SPONSORING/MONITORING  AGENCY  NAME(S)  AND  ADDRESS(ES) 

10.  SPONSOR/MONITOR'S  ACRONYM(S) 

11.  SPONSOR/MONITOR'S  REPORT 
NUMBER(S) 

12.  DISTRIBUTION/AVAILABILITY  STATEMENT 

Approved  for  public  release;  distribution  unlimited 

13.  SUPPLEMENTARY  NOTES 

14.  ABSTRACT 

see  report 

15.  SUBIECT  TERMS 

16.  SECURITY  CLASSIFICATION  OF: 

17.  LIMITATION  OF 
ABSTRACT 

18.  NUMBER 
OF  PAGES 

42 

19a.  NAME  OF 
RESPONSIBLE  PERSON 

a.  REPORT 

unclassified 

b.  ABSTRACT 

unclassified 

c.  THIS  PAGE 

unclassified 

Standard  Form  298  (Rev.  8-98) 

Prescribed  by  ANSI  Std  Z39-18 


Motion  Control  of 

Drift-Free,  Left -Invariant  Systems  on  Lie  Groups  * 


Naomi  Ehrich  Leonard^ 

Department  of  Mechanical  &  Aerospace  Engineering 
Princeton  University,  Princeton,  NJ  08544 
e-mail:  naomi@princeton.edu 

P.S.  Krishnaprasad 

Institute  for  Systems  Research  &  Department  of  Electrical  Engineering 
University  of  Maryland,  College  Park,  MD  20742 
e-mail:  krishna@isr.umd.edu 


IEEE  Transactions  on  Automatic  Control ,  Vol.40,  No. 9,  September  1995. 


Abstract 

In  this  paper  we  address  the  constructive  controllability  problem  for  drift-free,  left- 
invariant  systems  on  finite-dimensional  Lie  groups  with  fewer  controls  than  state  dimen¬ 
sion.  We  consider  small  (e)  amplitude,  low-frequency,  periodically  time- varying  controls  and 
derive  average  solutions  for  system  behavior.  We  show  how  the  pth-order  average  formula 
can  be  used  to  construct  open-loop  controls  for  point-to-point  maneuvering  of  systems  that 
require  up  to  (p4^1)  iterations  of  Lie  brackets  to  satisfy  the  Lie  algebra  controllability  rank 
condition.  In  the  cases  p  =  2,3,  we  give  algorithms  for  constructing  these  controls  as  a 
function  of  structure  constants  that  define  the  control  authority,  i.e.,  the  actuator  capabil¬ 
ity,  of  the  system.  The  algorithms  are  based  on  a  geometric  interpretation  of  the  average 
formulas  and  produce  sinusoidal  controls  that  solve  the  constructive  controllability  problem 
with  0(ep )  accuracy  in  general  (exactly  if  the  Lie  algebra  is  nilpotent).  The  methodology  is 
applicable  to  a  variety  of  control  problems  and  is  illustrated  for  the  motion  control  problem 
of  an  autonomous  underwater  vehicle  with  as  few  as  three  control  inputs. 
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1  Introduction 


Recent  work  in  nonlinear  control  has  drawn  attention  to  drift-free  systems  with  fewer  controls 
than  state  variables.  These  arise  in  problems  of  motion  planning  for  wheeled  robots  subject 
to  nonholonomic  constraints  [22,  23],  models  of  kinematic  drift  (or  geometric  phase)  effects 
in  space  systems  subject  to  appendage  vibrations  or  articulations  [12,  13],  and  models  of 
self-propulsion  of  paramecia  at  low  Reynolds  numbers  [26].  The  basic  state-space  model 
takes  the  form, 

m 

x  =  ^2  Fi(x)ui,  x  £  3?n,  Ui  £  3?,  m  <n  .  (1) 

2  =  1 

It  is  well  known  that  if  the  vector  fields  Fi  satisfy  a  Lie  algebra  rank  condition,  then  there 
exists  a  control  u  =  (iti, .  .  .  ,nm)  that  drives  the  system  to  the  origin  from  any  initial  state. 
However,  unlike  the  linear  setting  where  the  controllability  Grammian  yields  constructive 
controls,  here  the  rank  condition  does  not  lead  immediately  to  an  explicit  procedure  for 
constructing  controls.  As  a  result,  recent  research  has  focused  on  constructing  controls  to 
achieve  complete  controllability  [2,  14,  22,  23,  8,  20].  The  success  of  constructive  procedures 
based  on  periodically  time- varying  controls  [22,  23,  8,  20]  motivates  our  investigation. 

Our  interest  in  this  paper  is  in  constructive  controllability  using  periodic  forcing  of  drift- 
free,  left-invariant  systems  of  the  form 

m 

X  =  eXU,  U(t)  =  J2Aul(t),  (2) 

2  =  1 

evolving  on  matrix  Lie  groups.  Here,  X(t)  is  a  curve  in  a  matrix  Lie  group  G  of  dimension 
n,  U(t )  is  a  curve  in  the  Lie  algebra  Q  of  G,  m  <  n  and  {Ai, .  .  .  ,  An}  is  a  basis  for  Q. 
The  Lie  bracket  [•,  •]  on  the  matrix  Lie  algebra  Q  is  defined  to  be  the  matrix  commutator 
[. A ,  B]  =  AB<&BA,  for  A,B  £  Q .  (For  an  introduction  to  matrix  Lie  groups  and  Lie  algebras 
see  [6]).  The  u^(-)  are  assumed  to  be  periodic  functions  of  common  period  T.  e  is  a  small 
parameter  (0  <  e  <  1)  such  that  eui(-)  are  interpreted  as  small-amplitude  periodic  control 
inputs.  The  set  {Ai, .  .  .  ,  Am},  where  (iti, .  .  .  ,nm)  can  be  actuated  independently,  represents 
the  control  authority  of  the  system. 

Our  goal  is  to  solve  the  complete  constructive  controllability  problem  for  systems  of  the 
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form  (2)  which  can  be  stated  formally  as: 


(P)  Given  an  initial  condition  X,  £  G,  a  final  condition  Xf  £  G  and  a  time  tf  >  0,  find 
u{t)  =  ('Ui(t), .  .  . ,  um(t)),  t  £  [0,  tf],  such  that  Xf(0)  =  X{  and  X{tf )  =  Xf. 

Our  approach  is  to  derive  averaging  theory  for  systems  on  matrix  Lie  groups  of  the  form 
(2)  and  then  to  use  the  average  formulas  to  specify  open-loop  controls  that  solve  (P),  at 
least  approximately.  The  controls  are  designed  to  drive  an  average  system  solution  exactly 
thereby  driving  the  actual  system  approximately.  Open-loop  controls  can  be  used  to  exploit  a 
priori  knowledge  of  the  system  for  improved  system  performance  and  reduced  control  effort. 
Intermittent  feedback  can  then  be  used  in  conjunction  with  the  open-loop  control  to  reduce 
sensitivity  to  disturbances.  (For  related  ideas  see  [4]).  Feedback  control  laws,  including  time- 
varying  feedback  and  discontinuous  feedback,  have  been  studied  for  nonholonomic  systems 
([5,  25,  9,  27]). 

Equation  (2)  provides  a  general  framework,  or  normal  form,  for  a  class  of  systems  that 
includes  rigid  body  motion  control  problems.  For  many  of  these  problems,  the  system  config¬ 
uration  space  is  globally  described  by  a  matrix  Lie  group  making  (2)  a  natural  system  model. 
The  Lie  group  framework  then  leads  to  coordinate-free  expressions  for  system  behavior  and 
ultimately  to  coordinate-free  control  algorithms.  Further,  when  the  systems  on  Lie  groups 
are  left-invariant,  there  is  a  global-ness  to  our  solutions.  That  is,  even  if  we  exploit  local 
charts  to  make  small  maneuvers,  the  Lie  group  framework  allows  us  to  move  all  over  the 
configuration  space  without  reformulating  our  control.  This  is  because  we  can  always  treat 
the  current  position  of  the  system  as  if  it  were  the  identity  in  the  Lie  group. 

An  important  focus  of  our  work  is  to  exploit  the  Lie  group  structure  to  derive  formulas 
for  system  response.  Specifically,  we  show  the  utility  of  area  and  moment-like  expressions  in 
the  controls  and  structure  constants  of  the  Lie  algebra.  The  structure  constants  enable  us 
to  encode  control  authority,  thus  ensuring  that  our  results  naturally  account  for  changes  in 
control  authority  due  to  events  such  as  actuator  failures.  This  leads  easily  to  constructive 
procedures  for  on-line  adaptation  to  changes  in  control  authority. 
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Averaging  is  used  to  describe  an  approximate  solution  to  (2)  that  evolves  on  the  matrix 
Lie  group  G ,  remains  close  to  the  actual  solution  to  (2)  and  gives  rise  to  straightforward 
procedures  for  specifying  controls  to  address  (P).  Averaging  in  this  context  is  motivated  by 
the  work  of  Brockett  [3]  in  which  an  averaging  argument  was  used  to  describe  the  secular 
(linear  in  time)  motion  of  the  well-known  two-input  nilpotent  system  on  3?3  often  referred 
to  as  the  Brockett  system.  We  extend  the  argument  to  high-order  averages  and  to  systems 
on  finite-dimensional  Lie  groups. 

Liu  and  Sussmann  [30,  20]  also  develop  averaging  theory  to  derive  approximate  tracking 
control  for  drift-free  systems.  They  apply  averaging  theory  to  drift-free  systems  on  a  manifold 
M  with  highly  oscillatory  control  inputs.  Given  a  trajectory  of  a  suitable  “extended”  system, 
their  goal  is  to  find  a  trajectory  of  the  original  system  that  converges  to  the  given  trajectory 
and  use  this  result  to  derive  approximate  tracking  controls.  We,  on  the  other  hand,  do  not 
attempt  to  address  all  drift-free  systems,  but  rather  take  a  close  look  at  a  class  of  drift-free 
systems,  i.e.,  those  of  the  form  (2),  and  exploit  the  Lie  group  framework  as  described  above  to 
great  benefit.  Additionally,  while  Liu  and  Sussmann  consider  high-amplitude,  high-frequency 
control  inputs,  we  consider  small-amplitude,  low-frequency  control  inputs.  One  approach  is 
equivalent  to  the  other  by  scaling  time  by  e.  The  result  is  that  maneuvers  in  the  Liu  and 
Sussmann  time  scale  are  completed  in  one  unit  of  time,  while  in  our  time  scale  maneuvers 
are  completed  in  0(1 /e)  units  of  time.  However,  our  small-amplitude,  low-frequency  controls 
are  gentler  on  the  system  and  avoid  significant  off-course  excursions. 

Murray  and  Sastry  [22,  23]  and  Lafferiere  and  Sussmann  [14]  derive  control  inputs  to 
exactly  steer  drift-free  systems  that  can  be  transformed  into  a  nilpotent  form,  sometimes 
referred  to  as  “chained  form”.  Nilpotency  refers  to  the  fact  that  high-order  Lie  brackets  of 
vector  fields  are  identically  zero.  Our  Lie  group  framework  includes  the  case  of  nilpotent 
systems.  For  instance,  certain  chained-form  systems  can  be  represented  in  the  form  (2)  where 
the  Lie  group  G  is  unipotent,  i.e.,  is  upper  triangular  with  ones  along  the  diagonal,  and  the 
Lie  algebra  Q  is  strictly  upper  triangular  (nilpotent).  For  these  nilpotent  systems,  our  results 
provide  exact  steering  controls.  The  fourth  example  below  illustrates  a  chained-form  system 
put  in  the  form  of  (2). 
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There  are  special  cases  of  drift-free  systems  that  can  be  controlled  exactly  where  our 
methods  produce  only  an  approximate  solution.  For  instance,  in  [31],  Walsh  and  Sastry 
describe  a  method  to  derive  controls  to  exactly  orient  a  spacecraft  with  two  internal  rotors 
configured  about  two  of  the  principal  axes.  In  this  work,  however,  large  motions  are  necessary 
to  reorient  the  spacecraft.  We  emphasize  that  our  framework  is  more  general,  allowing  for  a 
large  class  of  systems  and  control  input  configurations  and  producing  controls  that  keep  the 
system  state  relatively  close  to  any  desired  path.  Further,  as  in  [12,  13]  our  solutions  give  a 
means  to  compute  drifts  in  system  behavior  caused  by  undesirable  oscillations.  Kinematic 
drift  of  a  spacecraft  caused  by  thermo-elastically  induced  vibrations  in  flexible  attachments 
on  the  spacecraft  is  an  example,  c.f.  [12,  13]. 

To  further  motivate  the  Lie  group  framework  we  give  four  examples. 

Spacecraft  Example:  Equation  (2)  describes  the  kinematic  spacecraft  attitude  control 
problem  if  we  interpret  U(t)  as  the  time-dependent  skew  symmetric  matrix  of  spacecraft 
angular  velocity  such  that  X  evolves  on  G  =  SO(3),  the  special  orthogonal  group,  where 

SO(k)  =  {Ae  $kxk\ATA  =  /,  det(A)  =  1}. 

Define  X(t)  £  S (9(3)  to  be  the  curve  of  rotations  that  maps  a  body-fixed  orthonormal 
coordinate  frame  into  an  inertial  coordinate  frame.  That  is,  xr  =  X(t)xb ,  where  Xb  is  any 
point  on  the  spacecraft  described  with  respect  to  the  body-fixed  frame  and  xr  is  the  same 
point  expressed  with  respect  to  the  inertial  frame.  Then  X(t)  describes  the  attitude  of  the 
spacecraft  at  time  t.  Define  * :  9?3  — >  so(3)  where  so(3)  is  the  space  of  3  X  3  skew  symmetric 
matrices  and  x  =  ( Xi,x2,x3)t  by 

0  4^33  x2 

x  =  x3  0  4^r;i  .  (3) 

<4^32  £l  0 

Let  ei  =  (1,0,  0)T,  e2  =  (0,1,  0)T  and  e3  =  (0,0, 1)T,  and  define  A{  =  e* ,  z  =  1,2,3.  Then 
{Ai,  A2,  A3}  is  a  (standard)  basis  for  Q  =  so(3)  and  X(t)  satisfies 

3 

x  =  xn,  n(t)  =  J2&l(t)Al  (4) 

2  =  1 
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where  fi  =  (fii,  fi2,  fis)T  is  the  angular  velocity  of  the  spacecraft  in  body-fixed  coordinates. 
Now  suppose  angular  momentum  of  the  spacecraft  is  conserved  and  equal  to  zero,  i.e., 
there  is  no  external  torque  applied  to  the  spacecraft.  Then  it  is  possible  to  interpret  the 
components  of  angular  velocity,  ,  as  our  small-amplitude,  periodic  controls,  e.g., 

eu{  =  0^,  i  =  1,2,3.  For  instance,  the  angular  velocities  could  be  effected  using  internal 
rotors.  Alternatively,  a  point  mass  oscillator  appended  to  the  spacecraft  could  be  used  to 
control  angular  velocity  (c.f.  [16]).  With  this  interpretation,  equation  (4)  takes  the  form 
of  (2)  with  G  =  S0(3),  n  =  3,  and  m  <  3  is  the  number  of  independent  actuators.  We 
note  that  any  control  configuration  can  be  represented  by  choosing  the  appropriate  basis 
for  so(3).  For  example,  suppose  there  are  only  two  independent  control  inputs  defined  by 
eui  =  fi3  +  fi2  and  eu2  =  fi2  +  fi3  (and  eu3  =  0).  Then  the  system  is  described  by  (2)  with 
{-Bi,  B2,  B3}  as  our  basis  for  so(3)  where  B3  =  A\  +  A2,  B2  =  A2  +  A3,  B3  =  A3.  Details  of 
averaging  and  constructive  controllability  applied  to  the  spacecraft  can  be  found  in  [18]. 

Unicycle  Example:  Equation  (2)  describes  the  motion  planning  problem  for  a  unicycle 
which  rolls  without  slipping  if  we  interpret  U(t)  as  the  appropriate  time-dependent  matrix 
of  steering  velocity  and  translational  velocity  such  that  X  evolves  on  G  =  SE( 2),  the  special 
Euclidean  group,  where 

SE(k)  =  {  ^  b  e  3?(fc+1)x(fc+1)|  A  e  SO(k),  b  G  SR*}. 

Here,  we  define  X(t)  G  SE(2)  to  be  the  planar  rigid  body  transformation  that  maps  a  body- 
fixed  orthonormal  frame  into  an  inertial  frame  so  that  X(t)  describes  the  position  at  time  t 
of  the  unicycle  in  the  plane  and  its  orientation  at  time  t  with  respect  to  an  inertially  fixed 
axis.  That  is,  for  Xb  a  point  on  the  unicycle  described  in  terms  of  body-fixed  coordinates 
and  xT  the  same  point  expressed  in  terms  of  inertial  coordinates,  [xr  1]T  =  X(t)[xb  1]T.  In 
terms  of  local  coordinates  ( x,y,8 )  where  {x,y)  describes  the  unicycle’s  position  and  8  the 
unicycle’s  orientation  on  a  plane  relative  to  the  inertial  frame,  X  can  be  expressed  as 


Suppose  that  u\  =  8  (steering  speed)  and  u2  =  v  (rolling  speed)  are  available  as  controls 
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and  let 

'  0  Ol  0  ]  [001' 

Ax  =  1  0  0  ,  Aa  =  0  0  0  ,  (5) 

_~0  0  0  J  [  0  0  |  0  _ 

and  A3  =  [A^A^].  Then  {A1,A2,A3}  defines  a  basis  for  se(2),  the  Lie  algebra  associated 
with  SE(2),  and  X{t)  satisfies 

X  =  eX^AyUy  +  A2U2)  (6) 

where  we  have  assumed  small-amplitude  controls.  Equation  (6)  is  of  the  form  (2)  with 
G  =  SE( 2),  n  =  3  and  m  =  2  and  takes  the  same  form  as  the  spacecraft  control  problem 
with  two  internal  rotors.  Details  of  averaging  and  constructive  controllability  applied  to  the 
unicycle  problem  can  be  found  in  [18].  There  it  is  illustrated  that  the  controls  derived  to 
steer  the  unicycle  are  identical  to  those  derived  to  control  the  spacecraft  with  two  internal 
rotors  as  a  result  of  the  two  systems  taking  the  same  form  (6). 

Underwater  Vehicle  Example:  Equation  (2)  describes  the  kinematic  motion  control  prob¬ 
lem  for  an  autonomous  underwater  vehicle  (AUV)  if  we  interpret  U(t)  as  the  appropriate 
time-dependent  matrix  of  vehicle  angular  and  translational  velocities  such  that  X  evolves 
on  G  =  SE( 3)  (see  [28]  for  another  study  of  an  AUV  on  SE( 3)).  In  this  case,  we  define 
X(t)  £  SE( 3)  to  be  the  rigid  body  transformation  that  maps  a  body-fixed  orthonormal  frame 
into  an  inertial  frame  so  that  X(t)  describes  the  position  and  orientation  in  three-dimensional 
space  of  the  underwater  vehicle  at  time  t.  Let 


Then  {Ai,...,A6}  defines  a  basis  for  Q  =  se(3),  the  Lie  algebra  associated  with  SE( 3). 
Now  let  Q  =  (fii,  Q2,  f^3)T  define  the  angular  velocity  of  the  vehicle  and  v  =  (iq,  U2,  u3)T 
the  vehicle  translational  velocity,  all  with  respect  to  the  body-fixed  coordinate  frame.  Then 
X(t)  satisfies 

x  =  x(£ni{t)Ai  +  5>_3(*)^).  (8) 

i— 1  i— 4 

We  assume  that  we  can  interpret  the  components  of  Q(t)  and  v(t)  as  controls  such  that  (8) 
is  of  the  form  (2),  e.g.,  let  eu{  =  i  =  1,2,3  and  eu{  =  Ui_3,  i  =  4,5,6.  In  this  case 
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G  =  SE( 3),  n  =  6,  and  m  =  6.  If  there  are  fewer  than  six  independent  actuators,  i.e., 
m  <  6,  then  some  of  the  eu{  are  identically  zero.  A  different  choice  of  basis  for  se(3)  and  a 
different  value  of  m  reflects  a  different  control  authority. 

Nilpotent  System  Example:  As  described  above,  systems  in  chained  form  can  also  typically 
be  put  in  the  form  of  drift-free,  left-invariant  systems  on  matrix  Lie  groups  (2).  As  an 
example,  consider  the  front-wheel  drive  car  which  can  be  transformed  (locally  about  the 
origin)  into  a  two-input  chained-form  system  on  9?4  [23]: 


Xi  =  vx 

x2  =  v2  (9) 

x3  =  x2v1 
X4  =  X3V4. 


This  system  can  be  expressed  (or  embedded)  as  evolving  on  the  matrix  Lie  group  consisting 
of  elements  of  the  form 

/I  x2  x3  x4  \ 

_  0  1  zi  * 

0  0  1  Xi 

V  0  0  0  1  / 

where  *  is  arbitrary.  A  basis  for  the  (nilpotent)  Lie  algebra  of  this  group  is  given  by 
{Ai,  A2,  A3,  A4 }  =  {Ai,  A2,  [A2,  Ai],  [[A2,  Ai],  Ai]}  where 


A\ 


(  0  0  0  0  \ 

0  0  10 

0  0  0  1 

V  0  0  0  0  / 


A2 


/  0  1  0  0  \ 

0  0  0  0 

0  0  0  0 

V  0  0  0  0  / 


Then  X(t)  satisfies 


X  —  eflf(Aini  T  A2v2) 


(10) 


which  is  of  the  form  (2)  with  n  =  4,  m  =  2,  where  we  have  assumed  small-amplitude 
controls.  Other  two-input  chained  form  systems,  such  as  the  kinematic  car  with  k  trailers, 
can  similarly  be  described  in  this  form  (c.f.  [16]). 


The  following  is  an  outline  of  the  remaining  sections  of  this  paper.  In  Section  2,  we 
state  some  preliminaries  including  definitions  of  geometric  objects  that  play  a  key  role  in 
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the  averaging  formulas  and  two  local  representations  of  the  solution  to  (2).  In  Section  3 
we  prove  second  and  third-order  averaging  theorems  for  systems  of  the  form  (2).  Our  main 
results  are  an  “area  rule”  for  second-order  averaging  and  a  “moment  rule”  for  third-order 
averaging.  A  statement  of  the  general  pth-order  averaging  theorem  is  given  in  Appendix 
A.  By  the  pth-order  average  solution  X^p\  we  mean  that  given  a  metric  d  on  the  Lie  group 
G,  d(X(t),  X(p\t))  =  0(ep),  Vt  £  [0,6/e],  b  >  0.  In  Section  4,  we  show  how  to  use  the 
average  formulas  for  (approximate)  constructive  controllability  by  deriving  controls  that 
steer  the  average  solution.  The  control  laws  become  increasingly  complex  for  increasing 
order  of  averaging,  and  so  we  seek  to  minimize  the  order  of  the  average  solution  that  we 
steer.  However,  a  sufficiently  high-order  average  solution  is  needed  in  order  to  capture  the 
controllability  of  the  system.  We  determine  pmin  where  pmin  is  the  smallest  p  such  that 
x(p)  can  be  driven  from  any  X{  £  G  to  any  desired  Xf  £  G  and  show  that  ( pmin  ^1)  is 
equal  to  the  highest  number  of  iterations  of  Lie  brackets  used  to  satisfy  the  controllability 
Lie  algebra  rank  condition.  The  proof  is  constructive  yielding  algorithms  that  produce 
continuous,  small-amplitude,  low-frequency,  open-loop  sinusoidal  controls.  The  algorithms 
are  driven  by  the  structure  constants  that  define  the  control  authority  and  controllability 
of  the  system.  In  Section  5  we  illustrate  the  algorithms  for  two  control  configurations  of  an 
autonomous  underwater  vehicle.  Conclusions  are  given  in  Section  6. 

The  results  of  this  paper  can  be  extended  to  the  setting  of  abstract  finite-dimensional  Lie 
groups  (c.f.  [16]).  To  keep  the  notation  simple,  we  stick  to  the  setting  of  matrix  Lie  groups. 
This  is  sufficient  for  our  examples. 

2  Preliminaries 

Our  average  solutions  X^  depend  on  the  geometric  objects  described  below.  We  make  the 
following  definitions  assuming  that  u(t)  is  periodic  in  t  with  period  T : 

'U'av  —  (‘U'avl;  •  •  •  j^awm)  ;  H  —  (Hi)  •  •  •  j  ^m)  ; 

1  yT  yt 

Uavt  =  —  /  Ui{r )dr,  u,(t)  =  /  u,(r)dT, 

1  Jo  Jo 
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So  u  =  u  and  if  uav 


m  m 

Uav  ^  ]  'U’avjA-i ,  JJ  ^  ]  'Uj  ‘  1  i  • 

2=1  2=1 

0  then  u  is  periodic  in  t  with  common  period  T. 


Assume  that  uav  =  0  and  define  Area,ij{T )  to  be  the  area  bounded  by  the  closed  curve 
described  by  Ui  and  Uj  over  one  period,  i.e.,  from  t  =  0  to  t  =  T.  By  Green’s  Theorem  we 
can  express  this  area  as 


Area. 


1  fT 

■ij(T)  =  ~  {ui(cr )uj(cr )  <^Uj(a)ui(a))da. 

Z  Jo 


(11) 


This  area  can  be  interpreted  as  the  projection  onto  the  i-j  plane  of  the  area  enclosed  by  the 
curve  (hi, .  .  .  ,hm)  in  one  period.  Define 

1  ft 

aii(t)  =  ~  ( Ui(a)uj(a )  <^Uj(a)ui(a))da. 

Z  Jo 

AreaiAT)t  ... 

A  ;  +m,  (i2) 


where  f{t  +  T)  =  f(t),  /( 0)  =  0.  Define 

1  [T 

mijk(T )  =  -  (ui(a)uj(a)  <^Uj(a)ui(a))uk(a)da.  (13) 

3  Jo 

Now  consider  the  closed  curve  C  defined  by  Uj{t )  and  uk{f)  over  one  period.  Let  A  be 

any  oriented  surface  with  boundary  dA  =  C .  Then  by  Stokes’  Theorem, 

mijk(T)  =  —  /  Ou,idujduk  <^Ujdukdui  +  2 ukduiduj.  (14) 

3  Ja 

So  nriijk{T )  as  described  by  (14)  can  be  interpreted  as  a  first  moment. 


The  average  approximation  X ^  will  also  depend  on  the  structure  constants  ,  associated 
to  a  given  basis  for  the  Lie  algebra  Q .  These  are  defined  by 

[Ai,  Aj]  =  J2  >  ijAk,  i,j  =  l,...,n.  (15) 

k= 1 

We  define  a  depth-fi  Lie  bracket  as  p  iterated  brackets,  e.g.,  a  depth-one  Lie  bracket  is  of 
the  form  [A,  B],  a  depth-two  bracket  is  of  the  form  [ A ,  [5,(7]]  or  [[A,  B],  (7],  a  depth-three 
bracket  is  of  the  form  [ A ,  [ B ,  [(7,5]]],  etc.,  where  A,  B,C,  D  £  Q.  A  depth-zero  bracket  is 
just  an  element  of  the  Lie  algebra  Q.  We  can  then  define  structure  constants  associated  to 
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higher  depth  brackets.  For  example,  we  define  depth-two  structure  constants  8^k  associated 
with  basis  {Ai, .  .  . ,  An}  according  to 

[[ A i,  Aj],Ak\  =  ,  \  -Ah  Ak\  =  ,  li:j[Ai,  Ak]  =  qlkAq  =  QqljkAq.  (16) 

1=1  1=1  q=l  1=1  q=l 

Skew  symmetry  of  the  Lie  bracket  on  Q,  [A,  B]  =  <^{B,  A],  implies  ,  =  44;  Further, 

Areaij(T )  =  ^AreajfT),  mijk(T )  =  ^mjik(T). 

Similarly,  the  Jacobi  identity,  [[A,  B],  C]  +  [[ B ,  C],  A]  +  [[(7,  A],  B]  =  0,  implies 

^jk  +  Q%i  +  @kij  =  0-  (17) 

Further, 

™>ijk{T)  +  mjkl(T)  +  mklj(T)  =  0.  (18) 

There  are  well-known  controllability  results  for  systems  on  Lie  groups  of  the  form  (2), 
e.g.,  [1,  29,  10].  We  use  [24]  as  a  convenient  reference.  Let 

C  =  {B  |  B  =  [ Bk ,  [Bk_ i,  [•  •  • ,  [B\,  Bo]  ■  ■  •]]],  Bi  G  {^4i,  •  •  • ,  Am},  i  =  0, . . . ,  k }.  (19) 

By  Proposition  3.15  of  [24],  for  G  a  connected  Lie  group,  if  Q  =  spanC  then  system  (2) 
is  controllable ,  i.e.,  a  solution  to  (P)  exists.  We  refer  to  this  condition  as  the  Lie  algebra 
controllability  rank  condition.  If  this  condition  is  satisfied  using  only  up  to  depth-j  brackets, 
i.e.,  k  <  j  in  (19),  then  we  say  that  system  (2)  is  a  depth-j  bracket  system. 

Since,  in  general,  there  are  no  explicit  global  representations  of  the  solution  to  (2)  we 
make  use  of  local  representations:  the  product  of  exponentials  representation  given  by  Wei 
and  Norman  [32]  and  the  single  exponential  representation  given  by  Magnus  [21].  We  begin 
by  defining  the  Wei-Norman  representation. 


Lemma  1  (Wei  and  Norman)  .  Let  X(t)  be  the  solution  to  (2)  with  X(0)  =  I.  Then 
3 >  0  such  that  for  |t|  <  to,  X(t)  can  be  expressed  in  the  form 


X(t) 


egi{t)Ai  eg2(t)A2  _  _  _  es in(t)An 


(20) 
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The  Wei-Norman  parameters  g  =  (gi, .  . .  ,  gn)T  satisfy 

g  =  eM(g)u  ,  for  \t\  <  to  ,  (21) 

where  g(0)  =  0  and  M(g)  is  a  real  analytic  matrix- valued  function  of  g.  If  Q  is  solvable  then 
there  exists  a  basis  of  Q  and  an  ordering  of  this  basis  for  which  (21)  holds  globally,  i.e.,  for 
all  t,  and  in  that  case  (21)  can  be  integrated  by  quadrature.  □ 

As  shown  in  the  work  of  Wei  and  Norman,  one  can  express  M(g)  of  (21)  in  terms  of  the 
structure  constants  of  (2).  For  ||g||  small, 

M(g)  =  I  +  ((g)  +  0(g2),  (22) 

where  the  ij th  element  of  ((g)  is 

4 is)  =  E  St,  l,  (23) 

k=j+ 1 

and  0(g 2)  are  higher  order  terms  in  the  gi. 

It  is  customary  to  refer  to  components  of  g  as  the  canonical  coordinates  of  the  second 
kind  for  G.  Let  W  be  the  largest,  connected  open  neighborhood  of  0  £  3?n  such  that  \/g  £  W, 
M(g )  is  well-defined.  Let  $  :  3?n  — >  G  define  the  mapping 

$(s)  =  e9lAle92A2  ■  ■  ■  e9nAn  (24) 

and  define  V  =  $(W)  C  G.  Then,  the  Wei-Norman  formulation  provides  a  local  repre¬ 
sentation  of  the  solution  to  (2)  for  initial  condition  X(0)  £  V  C  G.  Now  let  S  be  the 
largest  neighborhood  of  0  £  3?n  contained  in  W  such  that  \Er  =  <f,|s:S'— >Gis  one-to-one. 
Let  Q  =  ^(S)  C  V.  Then  :  S  — >  Q  is  a  diffeomorphism  and  we  can  define  a  metric 
d  :  Q  x  Q  — >  3?+  by 

d(Y,Z)  =  d(y-1(Y),y~1(Z))  (25) 

where,  for  ||  •  ||  a  norm  on  3?n,  d  :  3?n  X  3?n  — >  3?+  is  given  by 

d(a,(3)  =  || ck  0/3 1| .  (26) 
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As  an  alternative  to  using  the  Wei-Norman  representation  of  solutions  to  (2),  we  consider 
Magnus’  single  exponential  representation  [21].  By  Theorem  III  of  [21]  under  an  unspecified 
condition  of  convergence,  the  solution  to  (2)  with  .A(O)  =  /  can  be  expressed  as 

X(t)  =  ez(t)  (27) 

where  Z(t )  £  Q  is  given  by  the  infinite  series  (we  show  terms  up  to  0(e3)): 

Z(t)  =  ef  U(r)dT  +  %-  f  [U(t),U(t)\(1t 

Jo  Z  Jo 

+  J  jfljf  PM.  U(a)]da,  U(j)]iT  +  T  Jju(r),  [U(t),  U(r)]]dr  +  ...  (28) 

While  the  convergence  criterion  for  (28)  is  not  given  explicitly  in  [21],  two  different 
sufficient  conditions  are  provided  in  [11]  and  [7],  respectively.  Karasev  and  Mosolova  [11] 
show  that  (28)  converges  if 

J  \\adeU^\\d,T  <  ln2.  (29) 

For  G  a  finite-dimensional  Lie  group,  the  convergence  condition  (29)  is  equivalent  to 

/  || A(ett(r)) || c?r  <  ln2,  (30) 

Jo 

where  A(-)  is  an  n  X  n  matrix  with  ijth.  element  A ij(-)  defined  by 

n 

kj- 

k= 1 

In  the  case  that  G  =  SO(3)  and  {Ai,  A2,  A3}  is  the  standard  basis  for  Q  =  so(3),  it  is  easy 
to  compute  that  A(eu)  =  eU  and  so  (30)  is  equivalent  to 

/  || eZ7 (r) || <  ln2. 

Jo 

The  convergence  criterion  given  by  Fomenko  and  Chakon  [7]  takes  the  form 

£\\*U(T)\\dT  <  A  (31) 

where  M  >  1  is  defined  such  that  ||[A,  5]||  <  M||  A||  ||5||  for  all  A,  B  £  Q  and  the  universal 
constant  b  is  the  radius  of  a  disk  over  which  a  scalar  differential  equation,  defined  in  [7],  has 
an  analytic  solution. 


13 


Let  $  :  Q  — >  G  define  the  mapping 

n 

$(Z)  =  ez,  Z  =  (32) 

2  =  1 

Let  S  be  the  largest  neighborhood  of  0  £  Q  such  that  :  S  — >  G  is  one-to-one.  Let 

q  =  ns)  C  G .  Then  $  ;  5*  — >  Q  is  a.  diffeomorphism  andj  for  d  given  by  (26),  we  can  define 
a  metric  d  :  Q  x  Q  — »  9?+  by 

d(x,r)  =  d(^-1(x),^-1(r)).  (33) 

Following  Lazard  and  Tits  [15]  define  an  admissible  norm  on  Q  as  any  norm  ||  •  ||  that 
makes  (Q,  ||  •  ||)  a  Banach  space  and  satisfies 

\\[A,B]\\<\\A\\\\B\\,  VA,B  £  Q. 

Define  B(Q,  p)  =  {A  £  Q  |  ||A||  <  p}.  Then  from  Theorem  2.1  of  [15],  if  the  connected  center 
of  G ,  Cg0  j  is  simply  connected,  then  the  restriction  of  $  to  B{Q ,  7 r)  is  one-to-one.  Consider  a 
matrix  Lie  algebra  Q  C  3?nXn  and  the  induced  matrix  p-norm  ||  •  ||p  on  3?nXn.  We  can  always 
construct  an  admissible  norm  as  ||  •  ||l  =  2||  •  ||p,  since 

\\[A,B]\\l  =  2\\[A,B]\\P  =  2\\AB  O BA\\P  <  4||A||p-||5||p-  =  ||A||L||5||L. 

In  the  case  of  simply  connected  Cg0,  we  can  take  S  =  B(Q,ir)  =  {A  £  Q  |  ||A||i  <  7r }  = 
{A  £  Q  \  \\A\\  p  <  7r/2} .  The  condition  on  Cg0  holds,  in  particular,  for  finite-dimensional  Lie 
groups  with  trivial  centers  such  as  S0( 3),  SE( 2)  and  SE( 3).  Further,  for  simply  connected 
Lie  groups,  we  can  replace  7r  by  2ir,  i.e.,  we  can  take  S  =  B{Q ,  27t).  Thus,  for  all  these  kinds 
of  Lie  groups,  we  can  be  assured  that  our  norm  d  is  well-defined  on  a  significantly  sized 
neighborhood  of  the  identity  in  G. 

We  note  that  for  X  in  a  sufficiently  small  neighborhood  of  the  identity,  knowing  one  local 
representation  means  knowing  the  other  approximately  well. 

Lemma  2  Given  X  £  Q  fl  Q  C  G,  let  g  =  ^r_1(X)  and  Z  =  4r_1(X).  Then  g  =  0(ep)  if 
and  only  if  Z  =  0(ep),  p  >  1.  In  this  case,  \\g±  OZi]]  =  0[e2p ),  i  =  1, .  .  .  ,n. 
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Proof.  The  lemma  is  proved  by  expanding  exponentials  and  equating  the  two  local  represen¬ 
tations. 


3  Averaging 

Classical  averaging  theory  is  typically  applied  to  systems  evolving  on  3?n.  To  derive  averaging 
theory  for  systems  which  evolve  on  Lie  groups  (2),  we  apply  classical  averaging  theory  to  local 
representations  of  (2)  and  then  transfer  such  estimates  to  the  group  level.  The  theorems  in 
this  section  give  formulas  for  the  pth-order  average  solutions  X^p\t),  p  =  2,3.  For  illustration 
we  make  use  of  the  Wei-Norman  product  of  exponentials  representation  for  p  =  2  and  the 
Magnus  single  exponential  representation  for  p  =  3.  The  first-order  average  formula  can 
be  derived  to  be  X^(t)  =  X^(0)eeUavt .  This  describes  the  effect  of  the  DC  component 
of  U(t)  on  the  system.  This  is  useful  for  control  only  if  m  =  n.  As  a  result,  we  focus  on 
higher-order  average  formulas  which  capture  Lie  bracket  motion  of  the  system.  A  general 
pth-order  averaging  theorem  is  given  in  Appendix  A.  The  theorems  below  require  smooth 
controls;  however,  this  requirement  is  relaxed  in  the  appendix  where  piecewise  continuous 
controls  are  sufficient. 

We  note  that  these  theorems  state  that  the  formulas  are  valid  for  X(t)  in  a  neighborhood 
of  the  identity  of  G.  However,  because  system  (2)  is  left-invariant,  these  theorems  actually 
give  the  formulas  for  the  pth-order  approximation  X^O(t)  to  the  solution  X(t)  of  (2)  for  any 
initial  condition  A(0)  £  G.  Let  Xj(t)  and  XjP\t )  correspond  to  the  actual  and  approximate 
solutions,  respectively,  of  (2)  with  A/(0)  =  /  £  G.  By  left-invariance  of  (2),  X(t)  = 
A(0 )JW(t)  and  X(p\t)  =  X(0)X(jp)(t)  is  an  0(ep )  approximation  of  X(t). 

Theorem  3  (Second-Order  Averaging:  Area  Rule)  Consider  system  (2)  on  the  Lie 
group  G  with  Lie  algebra  Q.  Assume  that  U(t)  £  Q  is  periodic  in  t  with  period  T  and 
has  continuous  derivatives  up  to  third  order  for  t  £  [0,  oo)  and  assume  that  Uav  =  0.  Let 
D  =  {g  £  3?n  |  ||£||  <  r}  C  S  (where  r  >  0  is  chosen  as  large  as  possible).  Suppose  that 
A(0)  =  X0  £  Q.  Let  g{t )  be  the  solution  to  (21)  with  g(0)  =  g0  =  ^r_1(A0)  =  0(e).  Let 
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(34) 


ffo2>  =  (Siofi  ■  ■  ■  ,9no3))T  and  define 


t 


wk(t)  =  e2^  Areal0(T ),  £■  +  gk{0  \ 


gk  (t)  =  euk(t)  +  wk(t), 

r,W(t\A.  n^)( 


tiXt)  = 

X^2\t)  =  e9^’^^1  ■  ■  ■  e3";(t)-A™) 

where  ,  and  Area,ij{T )  are  defined  by  (15)  and  (11),  respectively.  If  ||g0 
and  if  g^2\t)  £  D,  X/t  £  [0,  6/e],  6  >  0,  then, 

d(X(t),XW(t))  =  0(e2),  Vt£  [0,6/e]- 


(35) 

(36) 
=  ^(e2) 


Proof:  Recall  by  (21)  and  (22)  that  for  small  ||g||, 

g  =  eM(g)u  =  eu  +  e£(g)u  +  eO(g2)u. 

By  second-order  classical  averaging  theory  (for  details  see  [16]), 

II g(t)  W2)WII  =  0(e2),  X/t  £  [0,  6/e], 

where 

g^2\t)  =  eu(t)  +  w(t) 

and  w(t)  is  the  solution  to 

^  Jq  i(u(cr))u(cr)dcr,  w(0)  =  g^ . 

From  the  definition  of  £  (23),  the  kth.  component  of  the  vector  £(u)u  is 


m  m 


^2Cki{u)ui  =  ^2  > 


i— 1 


i— 1  j=i+ 1 


(3T) 


(38) 


(39) 


So  using  integration  by  parts,  the  fact  that  ,  =  43$  =  Ui  and  the  definition  of  Areaij{T ) 

(11)  we  get  from  substituting  (39)  into  (38)  that 

t  rT™ 


£  PI 

Wk(t)  =  e2—  /  y'^l(fi(o-))nl(cr)dc7  +  5fc[,2) 

T  ^  i=i 


j.  m  m  njf 

=  4?.?  I  J  jp9‘j(p')^i(P'')d‘0'  “1“  9ko 

i=l  j=i+l  0 


=  e 


t  171  \  fT 

J,  Y,  2  Jo  ^(ct)%(ct))^,  *  +  5fco2) 


6 


=  e2^  5]  Aren^/T),  *  +  5fc[,2). 
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For  small  enough  e,  since  g^2\t)  £  D  C  S  then  g{t )  £  D  C  S,  Vt  £  [0,  6/e].  So  by  definition 
of  \H,  .A(t)  =  \Il(g(t))  and  X^2\t)  =  ^(^^(t)),  Vt  £  [0,6/e].  The  theorem  follows  by  (37) 
and  the  definition  of  d,  since  d(X(t),  X^2\t))  =  || g{t)  <t»5(2)(f) ||  =  0(e2),  Vt  £  [0,  6/e].  D 

We  show  further  in  the  next  proposition  that  the  structure  constants  ,  associated  to  a 
given  basis  for  Q  are  directly  related  to  the  Lie  brackets  of  the  vector  fields  defined  by  the 
columns  of  M(g)  evaluated  at  g  =  0. 


Lemma  4  Suppose  that  w(t)  is  defined  by  (34).  Let  [fi  f2  ■  ■  ■  fn]  =  M(g)  where  fk  is  the 
kth.  column  of  the  matrix  M(g).  Then 

€2t  m 

=  Y  ^  AreaiAT)[fi>  fj]\g=o  +  9o]-  (40) 

Proof:  By  (22)  and  (23)  we  have  that 


Efc=l+i  9k,  L  +  °(g2 


fi 


E2=i+i!ft,!r1)+o(s2) 

1  +  Yfk=i+ 1  9k,li  +  0(g2) 

EL,+i  gk,lf  +  0(g2) 


Efc=i+ 1  gk,  ki  +  0{g2 


So  for  i  <  j, 


9= 0 


which  by  (34)  completes  the  proof. 


M  f i  ~Mi  fi 

Qg  \g=0Ji\g=0  W  Qg  \g=oJj\g=0 


1  ' 

1  ‘ 

)  ji 

)  ij 

71 

71 

.  >  ji  - 

.  >  ij  . 

□ 


According  to  Theorem  3,  X^2\t)  can  be  expressed  as  a  product  of  exponentials  where  the 
exponents  have  an  0(e)  periodic  term  and  a  secular  term  (a  term  linear  in  t).  By  (34)  the 
secular  term  is  proportional  to  the  structure  constants  ,  and  the  projected  areas  Areaij{T ) 
bounded  by  the  closed  curves  described  by  Ui  and  Uj  over  one  period.  This  interpretation 
justifies  calling  Theorem  3  an  area  rule. 
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The  second-order  average  formula  derived  using  the  single-exponential  local  representa¬ 
tion  for  X(t)  (as  follows  from  Theorem  9,  Appendix  A)  takes  the  form: 

e2t  m 

42)«  =  ^k(t)  +  —  Areaij(T),  %  +  gk£\  (41) 

ZW(t)  =  zk(t)Ak,  X^2\t)  =  ezi2)W.  (42) 

k= 1 

A  comparison  of  the  two  second-order  average  formulas  shows  that  z^(t)  =  g^2\t). 

The  revealing  step  in  the  proof  of  the  single  exponential  area  rule  shows  that 

z(2\t)  = 

2±  pT  171  m 

=  2T I 

0  *=1  j= 1 

2  j.  n  m 

=  TE(  £  Area^T),  ^)Ak,  (43) 

k= 1  i,j=l\i<j 

This  result  confirms  that  the  formulas  X (2)  are  basis  independent.  Additionally,  (43)  reveals 
how  the  secular  term  in  the  second-order  approximation  captures  the  effect  of  the  group  level 
version  of  depth-one  Lie  brackets.  This  effect  is  developed  further  in  Section  4. 

Theorem  5  (Third-Order  Averaging:  Moment  Rule)  Consider  system  (2)  on  the  Lie 
group  G  with  Lie  algebra  Q.  Assume  that  U(t)  £  Q  is  periodic  in  t  with  period  T  and 
has  continuous  derivatives  up  to  fourth  order  for  t  £  [0,oo).  Further,  assume  that  Uav  =  0 
and  Area,ij{T)  =  0,  Vi,j.  Let  D  =  {Z  £  Q  \  \\Z\\  <  r}  C  S  (where  r  >  0  is  chosen  as 
large  as  possible),  b  is  defined  according  to  the  convergence  criterion  for  (28).  Suppose 
that  A(0)  =  X0.  Let  Z{t )  be  given  by  (28)  with  Z(0)  =  Z0  =  4r_1(A0)  =  0(e2).  Let 
43)  =  Eg=i  43E-  Define 

m  m  m  3/ 

43)W  =  +  J2  e2^W>  ij  «E  J2  jrm^k(T)eqljk  +  zqW,  (44) 

k=l 

Z{3\t)  =  ]Ez(q3)Aq,  X(3)(t)  =  ez(3)(t\  (45) 

q=l 

where  6qjk  and  nriijk{T)  are  defined  by  (16)  and  (13),  respectively.  If  \\Z0  ^Z^W  =  (9(e3) 
and  if  Z^3\t)  £  D,  Vt  £  [0,  6/e], 

d(X(t),XW(t))  =  0(e3),  Vf  £  [0,6/e]. 
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Proof:  By  classical  averaging  theory 


||Z(i)«Z<3>(i)||  =  0(£3),  Vie  [0, 6/e]  (46) 

where  (compare  with  formulas  (28)  for  Z(t)) 

Z<3>(i)  =  eU(t) +  j  J^[U(T),U(T)]dT f  [J^[U(cr),U(a)]da,U(T)]dT 
+  fffjf[i!(r),[U(T),U(T)]]iT  +  Z^ 

=  eU(t)+jJ\u(T),U(T)]dT+^jf{U(T),{U(T),U(T)]]dT  +  zl,a\  (47) 

The  second  equality  is  derived  by  integration  by  parts. 


By  definition,  we  have  that  eU(t)  = 
jJ‘{U,U](c)d 


-  Yl™=1  euq(t)Aq.  Following  the  steps  in  (43) 

n  m 

=  £  e2“ij(0>  h)Av 

q=l  i,j=l\i<j 


(48) 


The  third  term  on  the  right  side  of  (47)  can  be  expanded  as  follows: 


e3t  rT 


3  T 
3  T 


/  [C(t),[!7(t),£/(t)]]A 
Jo 


3j.  pT  m  m  m 

J  [J2uk(T)Ak,  [J^u^A^Y^Uj^Aj^dT 

*"*  k= 1  i=l  j=l 

^3^  m  m  y  pjf 

Z)  o  C^(T)%(T)  <^%(T)hl(T))hfc(T)dT[Afc,  [Ai,  Af\\ 


n  m  m  3j. 

=  «E(E  E  jT">«t(r)«;t)A,  (49) 

q  =  l  fc  =  l  = 

Therefore,  the  expression  for  Z^3\t)  given  by  (44)-(45)  is  verified.  For  small  enough  e,  since 
Z&\t)  C  S  then  Z(t)  e  D  C  S,  Vt  e  [0 ,6/e].  So  by  (27)  X(t)  =  ezW  =  i>{Z{t)).  The 
theorem  follows  by  definition  of  X^3\t)  and  d.  q 


The  third  term  on  the  right  side  of  (44)  is  a  purely  secular  term  proportional  to  the  first 
moments  nriijk{T )  and  the  depth-two  structure  constants  8^k  associated  with  choice  of  basis 
for  Q .  This  interpretation  makes  Theorem  5  a  moment  rule.  The  average  formula  is  clearly 
basis  independent. 
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Remark  6  Since  by  (17)-(18)  mijk(T)  +  mjki(T )  +  mkij(T )  =  0  and  6qjk  +  6qjki  +  =  0, 


TL  771  771  7~L  771  771 

EE  E  =  E  E  +  E  »m«(r))9?jt)A3. 

q=l  fc=l  i,j=l\i<j  q=l  i,j=l\i<j  k=i+ 1 

Substituting  this  into  the  area-moment  rule  (44)  incorporates  the  Jacobi  identity  and  removes 
redundant  terms.  This  is  significant  with  regard  to  constructing  controls  to  solve  (P). 

4  Constructive  Controllability 

The  strategy  that  we  propose  for  solving  (P)  approximately  can  be  summarized  in  four  steps: 

1.  Choose  intermediate  target  points  Xi,  X2, .  .  . ,  Xr  between  Xi  and  Xf  so  that  the  “dis¬ 
tance”  between  successive  target  points  is  small. 

2.  Specify  open-loop,  small-amplitude,  periodic  controls  that  drive  X(t)  from  Xi  to  the 
first  target  point  X\  approximately.  To  do  so,  specify  controls  that  drive  an  0(ep ) 
average  approximation  of  X(t)  from  Xi  to  X\  exactly  {jp  to  be  determined). 

3.  If  desired,  apply  feedback,  i.e.,  make  appropriate  modifications  based  on  measurement 
of  the  new  system  state.  For  example,  modify  selection  of  intermediate  target  points. 

4.  Repeat  steps  2  and  3  for  each  successive  target  point  (letting  the  previous  target  point 
be  the  new  initial  position)  until  done. 

The  fact  that  we  can  make  a  large  maneuver  by  repeating  our  technique  on  small  steps 
relies  on  the  left-invariance  of  our  system.  That  is,  we  can  always  reinitialize  at  our  current 
position  and  identify  it  with  the  identity  in  the  Lie  group. 

In  the  case  of  a  nilpotent  system,  Step  2  will  drive  X(t)  from  Xi  to  X\  exactly.  This  is 
a  result  of  the  fact  that  high-order  Lie  bracket  terms  are  identically  zero  (i.e.,  the  formula 
for  Z(t)  (28)  is  a  finite  sum),  and  so  an  appropriate  average  provides  an  explicit  solution  to 
(2).  The  proof  can  be  found  in  [16]. 
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For  Step  2,  we  use  the  average  formulas  of  the  previous  section.  To  determine  p  consider 
the  series  expansion  (28)  for  Z(t),  which  can  be  thought  of  (locally)  as  the  logarithm  of 
X(t).  One  can  observe  that  the  0(ep)  term  of  this  series  is  a  function  of  a  depth-(pOl)  Lie 
bracket.  Therefore,  one  expects  that  in  order  to  be  able  to  control  X ^  as  desired,  p  must 
be  greater  than  or  equal  to  pmin  where  (2)  is  a  depth-(pmin  <S-1)  bracket  system,  i.e.,  the 
controllability  rank  condition  is  satisfied  with  up  to  depth-(pmin  4AL)  brackets.  We  state  this 
formally  for  the  cases  p  =  2,3.  The  general  pth-order  case  is  given  in  Appendix  A. 

Theorem  7  Suppose  that  system  (2)  on  the  connected  Lie  group  G  is  a  depth-(p  ^>1) 
bracket  system,  p  =  2,  3.  Then  the  complete  constructive  controllability  problem  (P)  can  be 
solved  with  0(ep)  accuracy  using  the  formulas  for  X^k\t),  k  =  1, .  .  .  ,p,  and  p  is  the  smallest 
positive  integer  such  that  this  is  true. 

Proof.  The  proof  is  constructive  and  given  in  the  form  of  algorithms  that  synthesize  small- 
amplitude,  low-frequency,  continuous,  sinusoidal  controls.  Without  loss  of  generality  we 
assume  that  .A(O)  =  Xi  =  /  £  G  and  Xf  £  Q  fl  Q  C  G  is  such  that  gf  =  (gf1,  ■  ■  ■  ,gfn)T  = 
'F -\Xf)  =  0(^p~1'>)  and  Zf  =  ELi  ZfiA  =  ^~\Xf)  =  0(^p~1'>).  By  Lemma  2  = 

0(2(p  Ol)).  Therefore,  for  the  order  of  accuracy  of  control  that  we  seek,  gf  and  Zf  can  be 
used  interchangeably. 

The  algorithms  are  designed  to  solve  the  problem  X^p\tf)  =  Xf  by  solving  g^p\tf)  =  gf 
or  equivalently  Z^p\tf)  =  Zf.  Multiple  sub-steps  are  used.  That  is,  the  time  interval  [0 ,  t /] 
is  divided  into  subintervals,  e.g.,  [0 ,  t /]  =  [t0,  )  U  [^1,^2)  U  •••  U  t0  =  0,  = 

tf,  and  controls  specified  on  each  subinterval.  Because  as  assumed  above,  gf  =  (9(e(p-1)) 
and  Zf  =  O^-1)),  we  can  ensure  that  the  “initial  condition”  for  each  subinterval,  e.g., 
g{t 0),  g(t\),  5(^2))  etc.,  will  be  (9(e(p-1)),  i.e.,  will  satisfy  the  initial  condition  requirement 
for  the  averaging  theorems.  Thus,  the  appropriate  averaging  theorem  can  be  applied  to 
successive  subintervals.  Our  controls  will  be  specified  so  that  the  terms  uav ,  Area,ij{T )  and 
vnijkiT)  will  take  on  a  single  constant  value  on  each  subinterval.  However,  these  terms  may 
take  on  different  values  on  different  subintervals.  Thus,  for  ease  of  notation  we  define  the 
“running  total”  of  the  time- varying  area  terms  and  moment  terms  as  Area,ij{t )  and  nriijk{t ), 
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respectively.  Let  Area\^\T)  and  rn\^l(T)  be  the  values  of  the  area  and  moment  terms, 
respectively,  during  the  time  interval  [tT-i,tT]  and  suppose  that  t  £  [tv,tv+ 1],  0  <  v  <  g. 
Then  define 

Area^t)  =  ±  ^  Area^(T)  +  H^Are^T),  (50) 

r  —  1 

!j.\  ST'  (^r  ^tr-l)  (r)r  ^  (t  (^+1) 

y - m\jk{T)  +  - y - m\jk  V)'  (51) 

r  —  1 

Case  (i)  p  =  2.  Let 

=  {c  |  C  =  Ak  or  (7  =  [Aj  A?]j  hj>k  =  1, .  . .  ,  m}. 

The  definition  of  a  depth-one  bracket  system  implies  Q  =  spanC^1),  i.e., 

m  m 

Q  { ^  ^  ^  ^k  j  ^  3^} 

k= 1  a,  j  =  l 

m  m  n 

{  ^  >  Ck-A-k  +  ^  ^  ^  ^  j  C/-J  Cjj  G  3^}. 

Ai— 1  Ai— 1 

Therefore,  since  J2k=i  9fk^-k  G  there  exist  c^^Cij  G  3?,  j,  k  =  1, .  .  .  ,m  such  that 

n  m  n  m 

9fkAk  =  ^2  ckAk  +  ^2  ctL  ij-A-k-  (52) 

k=l  k=l  k=l  i,j=l\i<j 

By  Theorem  3, 

n  m  n  m  2j. 

Y^QkX^Ak  =  J2  ™k(t)Ak  +  J2  J2  -7^Areau(T)’  %A*-  (53) 

k= 1  k= 1  k= 1 

Thus,  to  find  controls  that  produce  g^2\tf)  =  g/,  we  equate  (52)  and  (53)  and  match 
coefficients.  That  is,  we  choose  uk{t ),  2  £  [0,  t /] ,  k  =  1, . .  .  ,m  such  that 

euk(tf )  =  Cfc,  k  =  1, .  .  .  ,m  and  (54) 

e2  Area,ij(tf)  =  Cij,  i,  j  =  1, .  .  .  ,  m,  i  <  j.  (55) 

Then  g^2\tf)  =  gf  so  that  A^2)(2/)  =  \H(gl2)(2/))  =  \H( gf )  =  Xf.  Thus,  by  Theorem  3, 

d(X(t,),X,)  =  <j(X(<,),  *<’>(<,))  =  0(e2). 

Algorithm  1  below  computes  uk{t ),  2  £  [0,2/],  k  =  l,...,m  such  that  (54)-(55)  are 
met.  This  is  done  by  recognizing  the  geometric  meaning  of  the  terms  Area,ij(T ),  i.e.,  that 
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Area,ij{T )  is  the  area  bounded  by  the  closed  curve  described  by  Ui  and  Uj  over  one  period. 
In  particular,  if  we  choose  U{  and  Uj  to  be  sinusoids  that  are  in  phase  then  Areaij{T )  =  0. 
Alternatively,  if  they  are  chosen  out  of  phase  then  Area,ij(T )  ^  0  is  a  function  of  the  signal 
magnitudes  and  phase  difference.  Based  on  this  reasoning,  the  final  values  of  each  of  these 
terms  can  be  matched  independently,  i.e.,  (54)-(55)  can  be  met.  The  timing  can  be  controlled 
by  choosing  the  frequency  and  amplitudes  of  the  sinusoids  appropriately.  In  Appendix  B 
we  define  two  algorithm  components.  The  first,  Component  l(i)  steers  Uk,  k  =  1, .  .  .  ,m  to 
satisfy  (54)  with  no  net  change  to  any  Areaij{t )  term.  The  second,  Component  l(ii)  steers 
area  terms  Area,ij{t ),  i,  j ,  i  <  j  to  satisfy  (55)  with  no  net  change  to  u.  In  each  component  the 
controls  have  an  initial  and  final  value  of  zero.  The  algorithm  components  are  like  computer 
subroutines  that  are  defined  once  and  for  all  and  called  as  necessary. 

ALGORITHM  1 


Choose  M  to  be  a  positive  integer  such  that  M  >  l/7re.  Let  the  period  T  and  frequency  co 
of  the  controls  be 

T  =  tf  =  — 

r(M  +  1)  +  1/2  ’  U  T' 

Then  using  the  controls  defined  in  Components  l(i)  and  l(ii),  perform  the  following  itera¬ 
tions: 
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1.  i  =  0. 

2.  2  =  2  +  1. 

3.  If  2  ^  S  go  to  5. 

4.  Apply  Component  l(ii)  for  c^-,  j  =  2  +  1, .  .  .  ,m. 

5.  If  2  <  m  ol  go  to  2. 

6.  Apply  Component  l(i)  for  ck,  k  =  1, . .  .  ,m. 


Then  we  are  done  and  d(X(tf),Xf )  =  (3(e2)  as  desired. 

Case  (ii)  p  =  3.  Let 

c(2)  =  {C  I  (7  =  Aq  or  (7  =  or  (7  =  [[A;,  A,-],  Afc],  q,i,j,k  =  l,...,m}. 


The  definition  of  a  depth-two  bracket  system  implies  that  (J  =  spanC^2),  i.e., 


Q  1  ^  '  C-qAq  +  ^  y  -  i  z  ■  -  i  j  +  ^  )  C'lj k:  i  z  ■  ‘  \j  •  '  i  A:  ■  +  ■  Cij  j  C'lj k:  ^  3^} 

9=1  *,j=l  i,j,k=l 


n  m 


m  m 


-  +  Y  C+>  ij  +  X]  X]  C^k^jk)Aq} 

q= 1  g=l  ;i< j  A:= 1  ;i< j 


n  m 


-  C9^9  +  Y  Y  (C+>  +  +  +  X]  C^k0^jk)Aq} 

9=1  g=l»,j=l;»<j  fc=i+l 

where  the  in  the  last  line  are  a  redefinition  taking  into  account  the  relations  among  8^-k 
induced  by  the  Jacobi  identity  (17).  Therefore,  since  Zf  £  Q ,  there  exist  cq,Cij,Cijk  £  3?, 
such  that 


n  m 


Zf  -  Y  C9^9  +  Y  Y  (C+>  ij  +  ^ji^iji  +  X]  C^^ljk)A 9 

g=l  g=l  =  /c=i+l 


(56) 


By  Theorem  5  and  Remark  6 


n  m 


e3t 


z{3\t)  =  J2  +  Y  Y  (e2aM’,!jOjrrnlJl(T)8x 

9=1  9=1  «,i=l;i<i 

m  e3t 

&  Y  -7f{2miok{T)  &mlkj(T))8qljk)Aq. 

k=i+ 1 


(57) 
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Thus,  to  find  controls  that  produce  Z^3\tf)  =  Zf ,  we  equate  (56)  and  (57)  and  match 


coefficients.  That  is,  we  choose  uq{t ),  t  £  [0,t/],  q  =  1, .  .  .  ,m  such  that 

euq(tf)  =  Cq,  q  =  l,...,m,  (58) 

e2ai3{tf)  =  eij,  =  (59) 

e3miji(tf)  =  =  and  (60) 

e3(2 mljk(tf)  <=t>mikj(tf))  =  <3cljk,  =  i  <  j,  k.  (61) 


Then  X^3\tf)  =  =  ty(Zf)  =  Xf  and  so  by  Theorem  5,  d(X(tf),  Xf)  = 

d(X(lt),xW(lt))  =  0(<?). 

Algorithm  2  below  computes  uq(t ),  t  £  [0 ,  t /] ,  q  =  such  that  (58)-(61)  are 

met.  This  is  done  by  recognizing  the  meaning  of  the  geometric  terms  as  in  Case  (i).  In 
particular,  the  terms  nriijk{T )  and  nriikj{T ),  i  <  j  <  k,  can  be  controlled  using  sinusoids  with 
1-2  resonance.  In  Appendix  B  we  define  Component  2(ii)  which  addresses  (59)  and  (60)  and 
Component  2(iii)  which  addresses  (61). 

ALGORITHM  2  Let 

£  =  {£  £  {m  +  1, .  .  .  ,n}  |  ,  ;  ^  0  some  i,  j  £  {1, .  .  .  ,m},  i  <  j}.  (62) 

Define  0  <  l  <  (n  <^>m)  by  l  =  |£|  =  number  of  elements  in  £.  We  will  assume  for  the 
purposes  of  the  algorithm,  without  loss  of  generality,  that  the  basis  {Ai, .  .  . ,  An}  is  chosen 
and  ordered  such  that  {Ai, .  .  .  ,  Am+/}  is  a  basis  for  A  +  [A,  A],  where  A  =  span{Ai, .  .  .  ,  Am}. 
Compute  cq,Cij,Cijk  such  that  (56)  holds  as  follows.  Consider  the  matrix 

"  nm-\-l-\- 1  /im+Z+ 1  /im+Z+ 1  /im+Z+ 1  /im+Z+ 1  /im+Z+1 

a121  U122  *  *  *  a12m  *  *  *  U1  mm  a232  *  *  *  (7(m-l)mm 

/im+Z+2  /im+/+2 

V121  Ui2 2  •  • 

6=  .  .  . 

An  An  An 

^121  ^122  '  '  '  (m  — l)mm  _ 

and  the  matrix 


m+1 

m+1 

m+1 

m+1 

m+1 

m+1 

j  12 
m+2 

J  13 

•  J  lm 

J  23 

•  J  2m  *  * 

*  >  (m  — l)m 

A 

J  12 

m-\-l 

m-\-l 

m-\-l 

m+Z 

m+Z 

m+Z 

i  12 

J  13 

•  J  lm 

J  23 

•  J  2m  *  * 

*  ?  (m  — l)m  _ 
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Let  a  =  m  +  l.  Then  8  has  rank  n  t)a  and  ,  has  rank  l.  Define  the  generalized  inverse  of  8 
to  be  8 *  =  8T (88T)~1 .  Let 

Cl21 
Cl22 

^(m  —  l)mm 

Define  the  generalized  inverse  of  ,  to  be  ,  t  =  ,  T(,  ,  T)~1.  Then  let 

'  z/„+1  «es=i;.<,G*03G  +  eek+i  -^C1) ' 

t 

J  .  J 

.  */„+,  «E”- +  EZUi  . 

m  m 

c*  =  zfqO  (c*j>  ij  +  c*J*6h  +  c*ok8%k)  q=l,...,m. 

i,j=l\i<j  k=i+ 1 

Y  —  {cijk  |  Cijk  7^0)  2  ^ 

Q  —  {Qjfc  G  F  |  i  <C  j  <C  A;},  /3  =  | <5 1  =  number  of  elements  in  Q , 

-R  =  {cijfc  G  F  I  k  =  i}  U  {cijk  £  Y  \  k  =  j  and  ^  F}, 

F  —  { Cij  |  Cij  F  0 )  ^  ^  J  } ) 

fF  =  {cijfc  G  A!  |  Cij  V},  S=\W\  +  \V\. 

Choose  M  to  be  a  positive  integer  such  that  M  >  l/7re.  Let  the  period  T  and  frequency  co 
of  the  controls  be 

T  =  _ t_l _  =  — 

(6/3  +  36)(M  +  1)  +  1/2’  U  T 

Then  use  the  controls  defined  in  Components  1  (i) ,  2(ii)  and  2(iii)  of  Appendix  B  to  perform 
the  following  iterations: 

1.  i  =  0. 

2.  i  =  i  +  1,  j  =  i. 

3-  j  =  j  +  1,  k  =j. 

4.  k  =  k  1.  If  k  =  m  +  1  go  to  8. 
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5.  If  cijk  =  cikj  =  0  go  to  7. 

6.  Apply  Component  2(iii)  for  Cijk  and  Cikj. 

7.  If  k  <  m  ol  go  to  4. 

8.  If  Cij  =  Ciji  =  Cijj  =  0  go  to  10. 

9.  Apply  Component  2(ii)  for  Cij,  c^i  and  Cijj. 

10.  If  j  <  m  Ol  go  to  3. 

11.  If  i  <  m  Ol  go  to  2. 

12.  Apply  Component  l(i)  for  cq  for  q  =  1, .  .  .  ,m. 

Then  we  are  done  and  d(X(tf),Xf )  =  (9(e3)  as  desired. 

The  proof  is  completed  by  noting  that  for  0  <  p'  <  p  and  p  =  2,3,  the  p'th-order  average 
solution  X(p')  captures  system  behavior  that  includes  only  up  to  {jp'  Ol)  Lie  brackets.  Thus, 
for  a  depth-(p  Ol)  bracket  system,  p  =  2,  3,  X^p )  cannot  be  controlled  as  desired. 

The  arguments  of  this  section  are  reminiscent  of  [1],  where  Brockett  works  with  piecewise 
constant  controls. 


5  Examples 


Consider  the  autonomous  underwater  vehicle  motion  control  problem  described  in  Section 
1.  The  Wei-Norman  equations  for  SE( 3)  with  our  chosen  basis  for  se(3)  are 
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The  parameters  gi ,  g2 ,  <73  correspond  to  Euler-angle  type  parameters  and  parametrize  the 
orientation  of  the  vehicle.  The  parameters  54,  55,  ge  parametrize  the  position  of  the  vehicle. 


AUV  with  Four  Controls:  First,  suppose  that  we  can  control  all  components  of  angular 
velocity  as  well  as  one  translational  velocity  component,  i.e.,  {Ai,  A2,  A3,  A4}  describes  our 
control  authority.  Then,  X{t )  describes  the  orientation  and  position  of  the  vehicle  and 
satisfies 

X  =  eX^u^Ai),  (64) 

2  =  1 

where  {Ai, .  .  .  ,  A6}  is  the  basis  for  se(3)  defined  by  (7).  This  system  is  a  depth-one  bracket 
system  since  [A3,  A4]  =  A5  and  [A4,  A2 ]  =  Ae.  We  have  n  =  6,  m  =  4,  and  the  nonzero 

structure  constants  corresponding  to  our  chosen  basis  for  se(3)  can  easily  be  computed  as 

3  _  1  _  2  _  1  6  _  5  _  1  6  4  _  1  5  _  4  _  1 

>  12  —  J  23  —  J  31  —  1>  J  15  —  J  61  —  1J  J  42  —  >  26  —  1J  >  34  —  >  53  —  L- 


Following  Algorithm  1,  we  compute 


) 


5  5  5  5  5  5 

J  12  J  13  J  14  J  23  J  24  >34 

6  6  6  6  6  6 

J  12  J  13  J  14  J  23  J  24  J  34 

0  0  0  0  0  1  ' 

0  0  0  0  Ol  0  ' 


0  0  0  0  0  1 
0  0  0  0  Ol  0 


Therefore, 


C24 

'  0 

Ol  ' 

*■■+» 

cn 

_ 1 

^9f6 

C34 

1 

0 

.  9f  6  . 

*■■+» 

cn 

1 _ 

Cl2  —  Ci3  —  Ci4  —  C23  —  0, 

Cl  =  gf1  OC24,  24  4^C34,  34  =  gf  1, 
2  2 

C2  =  9f  2  <^4>c24)  24  ■^>c34)  34  =  9f  2i 

3  3 

C3  =  9f  3  4^C24,  24  4^C34,  34  =  gf  3, 
ci  =  9f  4  4^C24,  24  4^C34,  34  =  g f  4, 

5  =  {2,3},  r  =  2. 

So  we  choose  an  integer  M  >  l/xe  and 


T 


_ _ 

2(M  +  1)  +  1/2’ 


uj  = 


2vr 

Y' 


Then  we  apply  Component  l(ii)  for  c24  followed  by  Component  l(ii)  for  c34  followed  by 
Component  l(i)  for  Ci,C2,c3,c4.  To  reduce  the  time  and  energy  expended  by  the  controls 
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Figure  1:  Control  Input  Signals  for  AUV  Example  with  Four  Controls. 

we  can  instead  apply  Component  1  (ii)  just  once.  To  do  this  let  i  =  4,  j  =  2,3,  c42  =  -wC24, 
c43  =  ■VVC34  and  apply  Component  1  (ii)  to  match  c42  and  c43.  In  this  case  we  have  S  =  {4} 
and  r  =  1  so  we  recompute 

^  tf  2?r 

M  +  3/2  T 

For  numerical  illustration,  let  e  =  0.1,  gf{  =  0.1,  i  =  1,...,6,  and  tf  =  23.  Choose 
M  =  10,  then  T  =  2,  lu  =  it .  Figure  1  shows  plots  of  the  corresponding  controls  eui,  eu2, 
eu3,  eu4  as  a  function  of  time.  Figure  2  shows  a  simulation  of  the  response  of  the  Wei-Norman 
parameters  g  as  a  function  of  time.  The  simulation  was  produced  by  numerically  solving 
the  equations  (63)  using  MATLAB.  The  horizontal  dashed  lines  of  Figure  2  represent  the 
desired  final  parameter  value  gf.  Figure  2  shows  that  g(tf )  03/  =  0(e2)  and  equivalently 
Z(tf)  <&Zf  =  0(e2)  as  expected.  By  the  results  of  Lazard  and  Tits  (see  Section  2)  for 
Q  =  se(3)  we  can  let  S  =  {A  £  se(3)  |  ||A||p  <  vr/2}  for  any  p.  From  Figure  2  it  is  clear 
that  Zf  £  S  and  Z(tf)  £  S  for  some  choice  of  p.  Thus,  \\Z(tf)  =  0(e2)  implies 

d(X(tf),Xf )  =  0(e2),  i.e. ,  the  AUV  has  been  repositioned  and  reoriented  as  desired  with 
0(e2)  accuracy. 

AUV  with  Three  Controls:  Now  consider  the  case  when  there  are  only  three  controls 
available,  e.g.,  suppose  that  due  to  an  actuator  failure,  the  third  component  of  angular 
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Figure  2:  Response  of  AUV  with  Four  Controls. 

velocity  can  no  longer  be  directly  actuated.  We  can  use  our  algorithms  to  adapt  on-line  by 
computing  new  controls  based  on  the  new  relevant  structure  constants.  Our  new  control 
authority  is  defined  by  {Ai,  A2,  A4}  and  X{t )  £  SE( 3)  satisfies 

X  —  cA^u^Ai  T  U2A2  T  U4A4),  (65) 


which  is  a  depth-two  bracket  system  since  [Ai,  A2]  =  A3,  [A4,  A2]  =  A6  and  [[Ai,  A2],  A4]  = 
A5.  For  the  purposes  of  the  algorithm,  we  reorder  our  basis  for  se(3)  such  that  A3  O  A4 
and  A5  O  A6.  The  nonzero  structure  constants  associated  with  this  reordered  basis  become 


4  —  1  —  2  —  1  5  —  6  —  1  5  —  3  —  1  6  —  3  —  1  -GVrfUpr  f )6 

)  12  —  )  24  —  )  41  —  )  16  —  )  51  —  )  32  —  )  25  —  )  43  —  )  64  —  V  UI UIICI ,  (7123 

n  =  6,  m  =  3,  and  so  by  (62),  £  =  {4,  5}  and  l  =  |£|  =  2.  Thus,  we  get 


1.  Thus, 


a  _  \q6  q6  r\&  r\&  n&  a&  q&  q&  q&  ] 

U  ~  L(7121  (7122  (7123  Ul3l  (7132  (7231  (7232  (7233j 

=  [0  0  1  0  0  0  Ol  0  0], 

4  4  4  1  r  i  o  o 

_  ?  12  J  13  5  23  _  1  u 

j  5  5  5  n  n  a 

?  12  5  13  J  23  J  L  U  U 

C123  =  Zf5, 

Cl21  =  Ci22  =  C231  =  C232  =  C233  =  C;L31  =  Ci32  =  Ci33  =  0. 

Note  that  Zf5  is  based  on  the  original  ordering  of  the  basis  for  se(3)  and  so  it  is  the  coefficient 
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of  A6  in  the  reordered  basis.  Further, 


Cl2 

[ 1  0  1 

Zf3  <^>C123^123 

Zf3 

C23 

o 

& 

1 _ 

zfe  4^C123^123 

^ zi% 

Cl3  =  0, 

Cl  =  Zf1  <^>(ci2,  12  +  C23,  23  +  C123&123)  =  Zf i> 

c2  =  Zf2  12  +  C23,  23  +  C123^?23)  =  Zf 2’ 

C3  =  Zf4  0(ci2,  12  +  c23j  23  C123$123)  =  Zf 4) 

Y_  =  {C123})  Q  =  {C123})  R  =  ty,  V  =  {C12,  C23} , 

W  =  0,  (3  =  1,  7  =  2. 

So  we  choose  an  integer  M  >  1/ire  and 

T  _  tf _  uj  =  — 

12(M  +  1)  +  1/2’  U  T' 

Then  we  apply  Component  2(iii)  for  C123,  followed  by  Component  2(ii)  for  Ci2,  followed  by 
Component  2(ii)  for  c23,  followed  by  Component  2(i)  for  Ci,C2,c3.  These  components  will 
specify  controls  ui,u2  and  u3.  However,  u3  is  really  our  original  control  n4  since  it  is  the 
coefficient  of  the  original  A4. 


For  this  particular  system  we  note  that  the  execution  of  the  algorithm  is  longer  than 
necessary,  i.e.,  there  are  steps  which  have  zero  net  effect  on  the  system.  Thus,  to  save  time 
and  energy  we  eliminate  the  unnecessary  steps  of  the  control  algorithm  defined  above.  The 
total  time  duration  of  the  parts  left  out  is  9 (M  +  1)T  so  we  recompute 

T  _  _ tt _  u  =  — 

3(M  +  1)  +  1/2  ’  U  T' 

For  numerical  illustration,  let  e  =  0.2,  tf  =  37  and  =  0.05,  gf2  =  0.05,  gf3  = 
0.04,  gf4  =  0.06,  gf5  =  0.05,  gf6  =  0.05  (recalling  from  Lemma  2  that  for  the  algorithm 
that  we  can  set  Zf  =  gf).  Choose  M  =  5,  then  T  =  2,  co  =  ir.  Figure  3  shows  plots  of  the 
corresponding  controls  eu4,  eu2  and  eu4  as  a  function  of  time.  Figure  4  shows  a  simulation 
of  the  response  of  the  Wei-Norman  parameters  g  as  a  function  of  time.  The  horizontal 
dashed  lines  of  Figure  4  represent  the  desired  final  parameter  values  gf.  Figure  4  shows 
that  g{tf )  <^3/  =  0(e3)  as  expected.  We  conclude  that  the  AUV  has  been  repositioned 
and  reoriented  as  desired  with  0(e3)  accuracy.  Reorientation  with  only  roll  and  pitch 
actuators  was  demonstrated  experimentally  using  this  algorithm  on  an  underwater  vehicle  in 
the  neutral  buoyancy  facility  of  the  Space  Systems  Laboratory  at  the  University  of  Maryland. 
For  details  see  [17]. 
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Figure  3:  Control  Input  Signals  for  AUV  Example  with  Three  Controls. 

6  Conclusions 

We  have  derived  average  formulas  for  the  solution  to  (2)  and  used  them  to  specify  small- 
amplitude  periodic  controls  that  solve  the  complete  constructive  controllability  problem  (P) 
approximately  (exactly  if  the  system  is  nilpotent).  We  have  shown  that  the  smallest  order 
of  the  average  formula  sufficient  to  solve  (P)  is  one  more  than  the  number  of  Lie  bracket 
iterations  needed  for  the  system  to  satisfy  the  Lie  algebra  controllability  rank  condition.  The 
results  were  developed  for  the  pth-order  average  approximation  where  p  =  2,3;  however,  the 
general  pth-order  average  theorem  is  stated  in  Appendix  A.  The  proof  of  the  controllability 
result  is  constructive  and  was  given  in  the  form  of  algorithms  for  generating  open-loop 
controls.  Structure  constants,  which  define  the  control  authority  of  the  system,  drive  the 
algorithms.  A  change  in  control  authority  such  as  an  actuator  failure  may  be  described  by  a 
change  in  structure  constants  and,  thus,  can  be  accommodated  on-line  using  the  algorithms. 
One  might  consider  the  algorithms  of  this  paper  to  be  a  “motion  script”  generator.  Thus,  a 
change  in  structure  results  in  a  change  in  script. 

Averaging  theory  for  systems  on  Lie  groups  also  holds  promise  for  understanding  and 
controlling  systems  with  drift.  That  is,  while  the  algorithms  derived  in  this  paper  are  valid 
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Figure  4:  Response  of  AUV  with  Three  Controls. 

only  for  drift-free  systems  of  the  form  (2),  the  averaging  theory  here  does  not  rely  on  the 
drift-free  assumption.  We  have  already  applied  our  averaging  theory  on  Lie  groups  to  the 
problem  of  controlling  a  class  of  switched  electrical  networks  which  can  be  modelled  as 
systems  on  Lie  groups  with  drift  [19]. 
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In  this  appendix  we  state  the  general  pth-order  averaging  theorem  and  constructive  con¬ 
trollability  theorem.  The  proofs  can  be  found  in  [16].  The  theorems  make  use  of  the  following 
recursive  formula  given  by  Fomenko  and  Chakon  [7]  for  the  terms  in  the  infinite  series  ex¬ 
pression  for  Z(t )  where  X(t)  =  ez W 


Theorem  8  (Fomenko  and  Chakon)  Let  8  =  b/M,  where  M  >  1  is  a  constant  such 
that  ||  [A,  B]  ||  <  M||A||||i?||,  \/A,B  £  Q  and  b  is  a  universal  constant.  Suppose  that  U(t) 
is  a  piecewise  continuous  curve  in  Q  and  /04  || eZ7 (r) || <  8.  Then  Z(t)  =  e%Zi(t)  is  a 

convergent  series.  The  terms  Z{(t)  are  uniquely  defined  by 

Z1(t)  =  T0(t)  =  fu{r)dT , 

Jo 

(i  +  l)Zi+i(t)  =  Ti  +  ^{  —  [ZT,  Ti-r]  (66) 

r  =  l  ^ 

+  ^2  k2q  ^2  [Zmi,  [Zm3,  ■  ■  ■  ,  [Zm2q,Ti-r\  .  .  .]}, 

q>l,2q<r  E=limj=r,mj>0 

Tk(t)  =  f  [Ufa),  f  [U(t2),...,[  U(LTk+1)dTk+1\dTk\...]dT1. 

Jo  Jo  Jo 

We  note  that  each  term  Z{  is  composed  of  depth-(i  Ol)  brackets. 


Theorem  9  (pth-Order  Averaging:  Area-Moment  Rule)  Consider  system  (2)  on  the 
Lie  group  G  with  Lie  algebra  Q .  Assume  that  U(t)  is  a  piecewise  continuous,  bounded  curve 
in  Q .  Let  b  >  0  be  such  that  /o  ||t7(r)||dr  <  8,  \/t  £  [0,  b],  where  8  is  as  defined  in  Theorem  8. 
Further,  assume  that  U(t)  is  periodic  in  t  of  period  T,  Vt  £  [0,  oo).  Let  p  >  1  be  an  integer. 
For  p  >  1  assume  that  Tk(T )  =  0,  k  =  0, .  .  . ,  p  02,  where  Tk(t )  is  defined  by  (66).  Suppose 
that  A(0)  =  X0  £  Q  C  G  is  such  that  Z0  =  4r_1(A0)  =  0(ep~1)  if  p  >  1  and  Z0  =  0(e)  if 
p  =  1.  Define 


Z{v\t)  =  J2W+1^Zl(t)  +  (^l)p+1ep^-Zp(T)  +  4P\  (67) 

2  =  1  -L 

X(p)(t)  =  ez(p)(t\  (68) 

where  Zi(t )  are  defined  by  (66).  If  ||Z0  ^Z^W  =  0(ep)  and  Z^p\t)  £  S,  Vf  £  [0,  b/e],  then 

d(X(t),X(p\t))  =  0(ep),  Vt  £  [0,  b/e].  (69) 
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Further,  for  t  =  NT,  N  an  integer, 

Z(p)(NT)  =  (ol  )p+1epNZp(T)  +  Z^\  (70) 

Remark  10  Further  explicit  decomposition  of  Z ^  into  terms  like  areas  and  structure  con¬ 
stants  can  be  found  in  [16]  imitating  the  argument  for  Z ^  in  (43)  and  Z ^  in  (49). 

Theorem  11  Suppose  that  system  (2)  on  the  connected  Lie  group  G  with  Lie  algebra  Q  is 
a  depth-p'  bracket  system.  Let  p  =  p'  +  1.  Then  the  complete  constructive  controllability 
problem  (P)  can  be  solved  with  0(ep)  accuracy  using  the  formulas  X^r\t)  given  by  (68)  for 
r  =  1, .  .  .  ,p.  Further,  p  is  the  minimum  positive  integer  such  that  this  is  true. 

Appendix  B 

In  this  appendix,  we  present  the  components  used  in  the  algorithms  of  this  paper.  The 
sinusoidal  controls  are  typically  sub-optimal.  However,  given  the  chosen  sinusoidal  structure 
of  the  controls,  the  amplitudes  are  selected  to  minimize  energy  (integral  of  sum  of  squares 
of  inputs).  In  the  following  control  laws,  if  a  control  component  is  not  explicitly  prescribed 
it  should  be  set  equal  to  zero. 

Component  l(i) 


Given:  ck,  k  =  1, .  .  .  ,m,  T,  co  and  current  time  t0. 

Goal:  Let  t\  =  t0  +  T/2.  We  define  eufc(f),  k  =  1, .  . .  ,m,  t  £  [t0,ti],  continuous,  such  that 
eufc(ti)  =  ck  and  euk{ti)  =  euk(t0 )  =  0. 

Controls: 

euk(t)  =  —ckujsin(uj(t  <^t0)),  t0  <  t  <  t\. 


Component  l(ii) 


Given:  Cij ,  for  i  and  j  =  i  +  1, .  .  .  ,m,  T,  co,  M  and  current  time  t0. 
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Goal:  Let  t1  =  t0  +  ( M  +  1  )T.  Let 


Oii  — 


OLA  — 


a.j'KM'' 


j  =i  +  1,- 


,  m. 


Specify  continuous,  zero-mean  controls  Ui{t ),  Uj{t ),  j  =  i  +  1, .  .  .  ,m,  t  £  [t0,ti]  such 
that  e2 Area,ij(ti)  =  Cij ,  eui(t0 )  =  eui(ti)  =  euj(t0 )  =  euj(ti)  =  0. 

Controls: 


eui{t )  =  c^o;  sin(o;(t  <^t0)) 

euj(t)  =  0 

eui{t )  =  ciiUJ  cos{uj{t  Osi)) 
euj(t)  =  ajo;  sin(o;(t  Osi)) 
eui{t )  =  ciiUJ  cos{uj{t  ^>s2)) 
euj(t)  =  0 


to  f;  t  <  to  +  if  —  51 


5i  <  t  <  5i  +  MT  =  s2 


s2  <  t  <  s2  +  —  ^1 


Note  that  e2Areaij(ti)  =  aiajirM  =  c^-  and  the  goal  is  met. 
Component  2(ii) 


Given:  i  <  j ,  c^-,  c^,  c^y,  T ,  u,  M  and  current  time  to- 
Goal:  Let  ti  =  to  +  3(M  +  1)T  and 


/  cij 

^2  2 

(  32c?..  \  1/6 

ai3  — 

(  32c?.. 

|  w 

V  ttM’ 

1  7T2M2  1  ’ 

^7T2M2 

Qjl  — 

CV7 

^2 

\  / 

2Ci7i 

aJ3  — 

OLiiirM  5 

CK{27rAf  5 

ol^ttM  * 

1/6 


Specify  continuous,  zero-mean  controls  'Ui(t)  and  Uj{t ),  t  £  [t0,  ti],  such  that  e3Area,ij(ti] 
c^,  e3niiji(ti)  =  t=Xiji,  e3mijj(t1 )  =  and  eu;(ti)  =  eui(t0 )  =  euj(ti)  =  euj(t0 )  =  0. 


Controls: 


eui{t )  =  sin(o;(t  <^t0)) 

euj(t )  =  0 

eui(t )  =  atla)cos(aj(t  ^i)) 
euj(t )  =  sin(o;(t  Osi)) 

eui{t )  =  cos(o;(t  ^52)) 

euj(t )  =  0 


T 

to  to  —  s\ 


5i  <  t  <  5i  +  MT  =  s2 


3  T 

$2  <  t  <  S2  ~\ - —  =  53 


39 


eui{t )  =  cti2LO  sin(o;(t  4453)) 
euj(t)  =  2 OLj2u  sin(o;(t  ^>s3)) 

eui{t )  =  cti2LO  cos (co(t  ^>s4)) 
euj(t)  =  2 aj2uj  cos(2 oj(t  4454)) 

eui{t )  =  cti2LU  cos(co(t  O55)) 
euj(t)  =  2 OLj2u  cos (w(t  O55)) 

eui{t )  =  2ctj3LU  sin(o;(t  4456)) 
euj(t)  =  cti3LU  sin(o;(t  4456)) 

eui{t )  =  2ctj3LU  cos(2 oj(t  4457)) 
euj(t)  =  cti3LU  cos(uj(t  O57)) 


r  _ 

■S3  <  t  <  S3  +  -  -  54 


54  ^  t  ^  54  T  M^T  —  55 


3  T 

55  <  t  <  55  -| - —  =  56 

T 

56  t  <  56  +  “  —  $7 


57  <  t  <  57  +  MT  =  58 


eui{t )  =  2aj3co  cos(co(t  4458)) 
euj(t)  =  cos(co(t  4458)) 


3  r 

58  t  s8  H - —  —  5g 

4 


The  condition  on  Area,ij{t )  is  met  during  the  time  interval  [0,  53],  the  condition  on  nriiji{t )  is 
met  during  [53,  56]  and  the  condition  on  nriijj{t )  is  met  during  [56,59]. 

Component  2(iii) 


Given:  i  <  j  <  k,  Cijk ,  c^-,  T,  o>,  M  and  current  time  t0. 


Goal:  Let  t3  =  t0  +  6(M  +  l)T.  Let  d3  =  2{\cijk  +  \cikj)/TrM  and  d2  =  2{\cijk  +  \cikj)  / -k  M . 
Select 


Pn=  (^1/3 


Pi  2  (  6 


1/3 


Pi\  ~ 
Pi2  = 


Pj  1 
d.2 


Pi  2 


1/2 

1/2 


^  =  7^? 

p^  =  kk- 


We  specify  continuous,  zero-mean  controls  Ui{t ),  'Uj(t)  and  uk{t ),  t  £  [t0,ti],  such 
that  €  (‘2iTYlijk(^t3'^  TYlikj  (^t  l))  —  "t-rCjj/c ,  C  {2>TTl{kj(^t  4)  44  (^1 ) )  —  44Ci/cj  ,  — 

eui(t0 )  =  euj(ti)  =  euj(t0 )  =  eufc(ti)  =  euk(t0 )  =  0.  Further,  Area^ti)  =  Area^ti)  = 

Areajfc(ti)  =  0. 

Controls: 


eui(t)  =  p^o;  sin(o;(t  44t0)) 
euj(t)  =  2pj1co  sin(o;(t  44to))  > 
eufc(t)  =  pfclo;  sin(o;(t  44t0)) 


T 

4  7  1  ^0  t  T 
4 


Sl 


eui(t)  =  p^o;  cos(o;(t  4454)) 
euj(t)  =  2pj1co  cos(2o;(t  4454))  > 

eufc(t)  =  pkluj  cos(uj(t  445i)) 


5i  <  t  <  5i  +  MT  =  s2 
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eui(t)  =  pi-yU  cos(uj(t  4452)) 
£Uj(t)  =  2pj1co  cos (co(t  4452)) 
euk(t)  =  pkluj  cos(uj(t  4452)) 


3  T 

>  52  <  t  <  52  H - —  —  53 


The  condition  on  nriijk{t )  is  met  during  the  time  interval  [0,  53].  However,  the  values  of  nriiji{t ) 
and  nrijkk{t )  at  t  =  53  may  be  different  from  their  initial  condition. 

So  repeat  the  controls  above  replacing  t0  with  5 3,  5i  with  54,  s2  with  s5  and  53  with  s6. 
Also,  replace  pjx  by  <^pj1  and  set  euk(t )  =  0,  t  £  [53,56].  During  [53,56],  the  original  value 
of  nriiji{t )  is  restored.  Repeat  the  controls  above  again,  this  time  replacing  t0  with  56,  5X 
with  57,  52  with  5s  and  53  with  59.  Also,  replace  pjx  by  <^pj1  and  set  eu{(t)  =  0,  t  £  [56,59]. 
Then,  during  [56,59]  the  original  value  of  nrijkk{t )  is  restored. 

Finally,  rerun  the  entire  set  of  controls  for  t  £  [to,  5g] ,  exchanging  the  roles  of  j  and  k, 
augmenting  the  indices  of  the  time  intervals  appropriately.  Also,  replace  pir  by  pi2,  pjx  by  pj2 
and  pkl  by  pk2.  Then  tx  =  5i8  and  e3(2mijk(t1)^mikj(t1))  =  cijk ,  e3(2mikj(t1)^mijk(t1))  = 
<^Cikj.  Thus,  the  goal  is  met. 
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